#include "servo_setting.h"

ServoSetting::ServoSetting(QObject *parent) : QObject(parent),
    rd_wr_lock_(QReadWriteLock::RecursionMode::Recursive)
{
    qvec_servo_enabled_.reserve(SERVO_NUM + 1);
    for (int i = 0; i <= SERVO_NUM; ++i) {
        qvec_servo_enabled_.push_back(false);
        gear_ratio_[i] = 1.0;
    }
    Q_ASSERT(qvec_servo_enabled_.size() == SERVO_NUM + 1);
}

ServoSetting *ServoSetting::get_instance()
{
    static ServoSetting instance;
    return &instance;
}

void ServoSetting::set_servo_gear_ratio(int slave_num, double gear_ratio)
{
    ASSERT_SERVO_NUM(slave_num);
    gear_ratio_[slave_num] = gear_ratio;
}

void ServoSetting::set_servo_gear_ratio(std::array<double, SERVO_NUM> gear_ratio_arr)
{
    Q_ASSERT(gear_ratio_arr.size() == SERVO_NUM);
    for (int i = 1; i <= SERVO_NUM; ++i) {
        gear_ratio_[i] = gear_ratio_arr[i - 1];
    }
}

void ServoSetting::set_servo_enable(int slave_num, bool enable)
{
    // if slave_num == 0, set all slaves
    if (slave_num == 0) {
        for (int i = 1; i <= SERVO_NUM; ++i) {
            qvec_servo_enabled_[slave_num] = enable;
        }
        return;
    }

    ASSERT_SERVO_NUM(slave_num);
    if (qvec_servo_enabled_[slave_num] == enable) {
        qDebug() << QString("WARN: Servo %0 has already been %1!!").arg(slave_num)
                    .arg(enable ? "enabled" : "disabled");
    } else {
        qvec_servo_enabled_[slave_num] = enable;
    }
}

bool ServoSetting::is_servo_enable(int slave_num) const
{
    ASSERT_SERVO_NUM(slave_num);
    return qvec_servo_enabled_[slave_num];
}

void ServoSetting::commit_setting_once()
{
    clear_fault_once_();
    set_enable_once_();
}

double ServoSetting::get_axis_actual_value(uint32_t axis) const
{
    Q_ASSERT(axis >= 1 && axis <= SERVO_NUM);
    QReadLocker locker(&rd_wr_lock_);
    return act_axis_value_[axis];
}

void ServoSetting::set_axis_target_value(uint32_t axis, double value)
{
    Q_ASSERT(axis >= 1 && axis <= SERVO_NUM);
    QWriteLocker locker(&rd_wr_lock_);
    req_axis_value_[axis] = value;
}

void ServoSetting::clear_fault_once_()
{
    if (!auto_clear_fault_) return;
    for (int i = 1; i <= SERVO_NUM; ++i) {
        if (ECAT->servo_state_fault(i)) {
            ECAT->set_servo_control_word(i, CW_FaultReset);
        }
    }
}

void ServoSetting::set_enable_once_()
{
    for (int i = 1; i <= SERVO_NUM; ++i) {
        if (qvec_servo_enabled_[i]) {
            // 使能
            if (ECAT->servo_state_switch_on_diabled(i)) {
                ECAT->servo_set_shutdown(i);
            } else if (ECAT->servo_state_ready_to_switch_on(i)) {
                ECAT->servo_set_switch_on(i);
            } else if (ECAT->servo_state_switched_on(i)) {
                ECAT->servo_set_enable_operation(i);
            } else if (ECAT->servo_state_operation_enabled(i)) {
                // nothing
                // this is the target state
            } else {
                // unknown states
            }
        } else {
            // 取消使能
            if (ECAT->servo_state_switch_on_diabled(i)) {
                ECAT->servo_set_shutdown(i);
            } else if (ECAT->servo_state_ready_to_switch_on(i)) {
                // nothing
                // this is the target state
            } else if (ECAT->servo_state_switched_on(i)) {
                ECAT->servo_set_shutdown(i);
            } else if (ECAT->servo_state_operation_enabled(i)) {
                ECAT->servo_set_disable_operation(i);
            } else {
                // unknown states
            }
        }
    }
}

void ServoSetting::update_axis_actual_value_()
{
    for (int i = 1; i <= SERVO_NUM; ++i) {
        act_axis_value_[i] = ECAT->servo_state_actual_pos(i) / gear_ratio_[i];
    }
}

void ServoSetting::set_to_ecat_axis_target_value_()
{
    for (int i = 1; i <= SERVO_NUM; ++i) {
        ECAT->set_servo_target_position(i, req_axis_value_[i]);
    }
}
